#!/usr/bin/env python
# -*- coding:utf-8 -*-

from queue import PriorityQueue
from planner.planner import Planner

class AStarPlanner(Planner):
    """
    参考：https://www.redblobgames.com/pathfinding/a-star/introduction.html
    """
    def __init__(self, graph=None, start=None, goal=None):
        super(AStarPlanner, self).__init__(graph, start, goal)
        self.cost_so_far = {}
        self.came_from = {}

    def _reset(self):
        self.came_from[self.start] = None
        self.cost_so_far[self.start] = 0

    def plan(self, graph=None, start=None, goal=None):
        print("plan in graph {} from {} to {}".format(graph, start, goal))
        if not self.check(graph, start, goal):
            return None
        # print("24 nodes", graph.nodes())
        self._reset()
        prio_queue = PriorityQueue() #优先队列，默认升序 open
        prio_queue.put((0, start))
        while not prio_queue.empty():
            prio, cur_nid = prio_queue.get()
            if self.reach_goal(cur_nid):
                self.generate_path(cur_nid)
                print("32, reach goal, and finish!")
                break
            successor = self.successor(cur_nid)
            # print("35 sucs", cur_nid, successor)
            for succ in successor:
                new_cost = self.cost_so_far[cur_nid] + self.cost(cur_nid, succ)
                if succ not in self.cost_so_far or self.cost_so_far[succ] > new_cost:
                    self.cost_so_far[succ] = new_cost
                    prio = new_cost + self.heuristic(succ, self.goal)
                    self.came_from[succ] = cur_nid 
                    prio_queue.put((prio, succ))  #TODO，if suc in priority queue need update
        return self.result
    

if __name__ == "__main__":
    pass